/*
	2007-2008 UMKC Robotics Team
	ATMega128 (CPU1) Code
	Updated: 2/1/08
	
	Created By: Jared Bayne
	E-Mail:		JaredNBayne@gmail.com
	Created On:	February 1st, 2008

	Purpose:	This program is for the 2007-2008 IEEE
				Region 5 Student Robotics Competition.


*/


/*

ToDo List:

 - Remove pereller board
 - wire mouse(s) to the alt-mega128
 - programming alt-mega128 for movement
 -- Tracking Rotation
 -- set unsafe zones
 -- set boundries of field
 

*/


#define	DEBUG	0		//1=yes	0=no
#define SIMULATE_COLOR	1


int Do_Wireless_Start=0;
int regular_speed=100;  


#define	red			0
#define	green 		1
#define	yellow 		2

#define	full	1
#define	empty	0

#include <avr/io.h>
#include <util/delay.h>
#include <avr/interrupt.h>
#include "LCD.h"
#include "USART.h"
#include "Initialize.h"
#include "string.h"
#include "motors.h"
#include "RC_Control.h"
#include "buttons.h"
#include "propellor_comms.c"


const unsigned char Splash[] = "Splash";

void Init_Goes(void);
void Init_Speed(void);
#define LED	   GET_BIT(PORTA).bit6	//Notice, output uses PORTB

int main (void) 
{

	/* Initializations */
	//_delay_ms(3000);
	InitPorts();
	InitUSART1();
	InitUSART0();
	InitLCD();
	InitMotors();
	writeln(Splash);
	USART1writeln(Splash);
	LED=ON;	
	
	
	/*while(1)
	{
	
		fill_coordinates_debug();
		_delay_ms(250);
	
	}*/
	
	map_course(regular_speed);	//use this function to manuall run bot around course and take measurements
								//use the buttons on the bot to move it around...LCD will display coordinates


///************Example of basic movements*******************************/
//
//	int disp_counter=0;
//	writeln("Going FOrward");
//	while(mouse_y<1000)
//	{
//		goforward(regular_speed);	//go forward until we are at the blue box
//		fill_mouse_positions();
//		
//		//here on down is for debugging
//		disp_counter++;
//		
//		if (disp_counter>20000)
//		{
//			LCDLine2();
//			LCDDispInt(disp_counter);
//			display_mouse_values();
//			disp_counter=0;
//		}
//		
//	}
//
//	write("Going Backward");
//	while(mouse_y>0)
//	{
//		gobackward(regular_speed);	//go forward until we are at the blue box
//		fill_mouse_positions();
//		disp_counter++;
//		if (disp_counter>20000)
//		{
//			display_mouse_values();
//			disp_counter=0;
//		}
//		
//	}
//	
//	stop();
//	
//	while(1);
//
///*********End Example of Basic Movements******************************/
//
//
//
//
//
//
//
///**********Example of moving from center to box 1*********************/	
//	
///**Note
//	This function basically says, " Go to coordinate (X,Y)". 
//	Where X is the value in the first loop and Y is the
//	value in the second loop.
//	
//**/	
//	disp_counter=0;
//	fill_mouse_positions();		//update mouse1_x and mouse1_y variables
//	display_mouse_values();		//display them on the LCD screen
//	while(mouse_x<160)
//	{
//		fill_mouse_positions();	//update the variables every loop iteration
//		goright(regular_speed);	//turn right until we are lined up with box 1
//		
//		
//		//
//		disp_counter++;
//		if (disp_counter>20000)
//		{
//			display_mouse_values();
//			disp_counter=0;
//		}
//	}
//	display_mouse_values();		//display mouse1_x and mouse1_y...mouse1_x should be equal to the
//								//value in the loop above
//	
//	//reset_mouse_positions();	//reset function isn't quite working yet (look at it)
//	disp_counter=0;
//	while(mouse_y<3500)
//	{
//		goforward(regular_speed);	//go forward until we are at the blue box
//		fill_mouse_positions();
//		disp_counter++;
//		if (disp_counter>20000)
//		{
//			display_mouse_values();
//			disp_counter=0;
//		}
//		
//	}
//	
//	stop();	//at box
//	
///**************At box 1*****************************/
//	
//	
	
	while(1)
		RC_Control(250);
	
	
	
//	Disp_FLS();
	stop();
	
	


}

//void Init_Goes(void)
//{
//
//	if (ROUNDNUMBER==1)
//	{
//		heavy_goes	=	room_one;
//		medium_goes	=	room_two;
//		light_goes	=	room_three;
//	}
//	else
//	{	
//		heavy_goes	=	red;
//		medium_goes	=	green;
//		light_goes	=	yellow;
//	}
//		
//}

//void Init_Speed(void)
//{
//
//	if (ROUNDNUMBER==1)
//		Approach_Stupid_Room=regular_speed;
//	else
//		Approach_Stupid_Room=regular_speed-45;
//
//
//}



